/**
 * @file:          CloseLoopV1_2.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.09
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.09,14:49:24
 */
/* Include Files **************************************************************/
#include "CloseLoopV1_2.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
uint8 close_loop_v1_2_Init(CloseLoopV1_2 *self)
{
    self->LedToggle = close_loop_v1_2_LedToggle;
    self->AdcCalibration = close_loop_v1_2_AdcCalibration;
    self->Adc2Current = close_loop_v1_2_Adc2Current;
    self->GetIaFdb = close_loop_v1_2_GetIaFdb;
    self->GetIbFdb = close_loop_v1_2_GetIbFdb;
    self->UpdatePWM = close_loop_v1_2_UpdatePWM;

    self->led_toggle_period_count = (uint32)(self->param_led_toggle_period_s / 2 / self->mcu->param_task_period_s);
    // self->mcu->GpioToggle(self->param_pin_LED_in.pin);
    // DBprintf("asdasd\r\n");

    // self->mcu->SetPwm(self->mcu, 5, 0.5);
    // self->mcu->SetPwm(self->mcu, 6, 0.5);

    return TRUE;
}

void close_loop_v1_2_LedToggle(CloseLoopV1_2 *self)
{
    // printf("asd\r\n");
    // self->mcu->GpioToggle(self->param_pin_LED_in);

    // if (self->GetIaFdb(self) > 0)
    // {
    // DBprintf("ll:%d\r\n", self->led_toggle_period_count);
    // DBprintf("l:%d\r\n", self->led_toggle_count);
    // DBprintf("l:%d\r\n", self->param_pin_LED_in.pin);
    if (self->led_toggle_count % self->led_toggle_period_count == 0)
    {
        // DBprintf("----:%d\r\n", self->led_toggle_count);
        self->mcu->GpioToggle(self->param_pin_LED_in.pin);
    }
    self->led_toggle_count++;
    // }
    // else if (self->GetIaFdb(self) < 0)
    // {
    //     self->mcu->GpioSet(self->param_pin_LED_in);
    //     self->mcu->DelayMs(self->mcu, 250);
    //     self->mcu->DelayMs(self->mcu, 250);
    //     self->mcu->DelayMs(self->mcu, 250);
    //     self->mcu->DelayMs(self->mcu, 250);

    //     // self->mcu->GpioToggle(self->param_pin_LED_in);
    //     self->mcu->GpioReset(self->param_pin_LED_in);
    //     self->mcu->DelayMs(self->mcu, 250);
    //     self->mcu->DelayMs(self->mcu, 250);
    //     self->mcu->DelayMs(self->mcu, 250);
    //     self->mcu->DelayMs(self->mcu, 250);
    // }
    // else
    // {
    // }
}

uint8 close_loop_v1_2_AdcCalibration(CloseLoopV1_2 *self)
{
    uint16 i = 0;
    int32 Ia_sum = 0;
    int32 Ib_sum = 0;

    for (i = 0; i < self->param_adc_calibration_buffer_size; i++)
    {
        self->mcu->ADC_StartConversion();
        self->mcu->ADC_WaitConversionComplete();
        Ia_sum += self->mcu->AdcGetResult(ADC_NUM1, 1);
        Ib_sum += self->mcu->AdcGetResult(ADC_NUM2, 1);
    }

    self->param_Ia_adc_offset = Ia_sum / self->param_adc_calibration_buffer_size; // - self->mcu->adc1_offset;
    self->param_Ib_adc_offset = Ib_sum / self->param_adc_calibration_buffer_size; // - self->mcu->adc2_offset;

    return 1;
}

float32 close_loop_v1_2_Adc2Current(CloseLoopV1_2 *self, int16 adc_val)
{
    return (float32)(adc_val) / 4096 * self->param_adc_ref_V / self->param_amp_gain / self->param_R_of_current_sample_ohm;
    // return (float32)(adc_val) / 4096 * self->param_adc_ref_V;
}

float32 close_loop_v1_2_GetIaFdb(CloseLoopV1_2 *self)
{
    float32 out = 0;

    self->Ia_adc_value = self->mcu->AdcGetResult(ADC_NUM1, 1) - self->param_Ia_adc_offset;
    out = self->param_current_dir * self->Adc2Current(self, self->Ia_adc_value);

    return out;
}

float32 close_loop_v1_2_GetIbFdb(CloseLoopV1_2 *self)
{
    float32 out = 0;

    self->Ib_adc_value = self->mcu->AdcGetResult(ADC_NUM2, 1) - self->param_Ib_adc_offset;
    out = self->param_current_dir * self->Adc2Current(self, self->Ib_adc_value);

    return out;
}

void close_loop_v1_2_UpdatePWM(CloseLoopV1_2 *self)
{
    if (self->param_pwm_output_en == TRUE)
    {
        self->mcu->SetPwm(self->mcu, 1, self->in_pwm_duty_A1);
        self->mcu->SetPwm(self->mcu, 2, self->in_pwm_duty_A2);
        self->mcu->SetPwm(self->mcu, 3, self->in_pwm_duty_B1);
        self->mcu->SetPwm(self->mcu, 4, self->in_pwm_duty_B2);
    }
    else
    {
        self->mcu->SetPwm(self->mcu, 1, 0);
        self->mcu->SetPwm(self->mcu, 2, 0);
        self->mcu->SetPwm(self->mcu, 3, 0);
        self->mcu->SetPwm(self->mcu, 4, 0);
    }
}
